/**
 * 约束类
 */

 import * as THREE from "./three.js/build/three.module.js";
 import A3Actor from "./A3Actor.js";
 import { 
  SPI, HPI, DPI, 
  AxisX, AxisY, AxisZ,
  isNull,
  toTransform,
  updateTransform,
  createHingeConstraint, 
  create6DofConstraint,
  createSliderConstraint,
  toAmmoType
} from "./A3Common.js";

class A3Constraint extends A3Actor {
  name = 'constraint';

  disableCollisionsBetweenLinkedBodies = true;
  useReferenceFrameA = true;

  bodyA = undefined;
  bodyB = undefined;
  constraint = undefined;

  constructor( bodyA, bodyB ) {

    super();

    this.bodyA = isNull( bodyA ) ? new Actor() : bodyA;
    this.bodyB = isNull( bodyB ) ? new Actor() : bodyB ;
    this.constraint = undefined;

    this.frameInA = toTransform();
    this.frameInB = toTransform();

  }

  createConstraint(){ console.warn("create constraint is not implemented.") }

  enableConstraint( enable=true ) {

    if( enable ) {

      // 移除已有约束
      if( !isNull(this.world) && !isNull(this.constraint) ) {
        this.world.removeConstraint( this.constraint );
      }

      // 生成新约束
      this.createConstraint();

      if( !isNull( this.constraint ) && !isNull(this.world) ) {
        this.world.addConstraint( this.constraint, this.disableCollisionsBetweenLinkedBodies );
      }

    }
    else {

      if( !isNull(this.world) && !isNull(this.constraint) ) {
        this.world.removeConstraint( this.constraint );
      }

    }

  }

  setBodyA( pivot, axis ) {
    this.updateFrameInX( pivot, axis, true );
  }

  setBodyB( pivot, axis ) {
    this.updateFrameInX( pivot, axis, false );
  }

  updateFrameInX( pivot, axis, isBodyA=true ) {

    const frame = isBodyA ? this.frameInA : this.frameInB;

    if( isNull( pivot ) ) {
      pivot = new THREE.Vector3( 0, 0, 0 );
    }

    if( isNull( axis ) ) {
      axis = new THREE.Vector3( 0, 0, 1 );
    }

    const quaternion = this.calculateQuaternion( axis );

    updateTransform( frame, pivot, quaternion );

  }

  // 计算从旋转轴Z到目标旋转轴的旋转四元数
  calculateQuaternion( toAxis, fromAxis ) {
    
    // todo: 使用数学方式计算四元数的值
    const tmpQuat = new THREE.Quaternion( 0, 0, 0, 1 );

    if( toAxis.x === 1 ) {
      tmpQuat.setFromAxisAngle( AxisY, HPI );
    }

    if( toAxis.x === -1 ) {
      tmpQuat.setFromAxisAngle( AxisY, -HPI );
    }

    if( toAxis.y === 1 ) {
      tmpQuat.setFromAxisAngle( AxisX, -HPI );
    }

    if( toAxis.y === -1 ) {
      tmpQuat.setFromAxisAngle( AxisX, HPI );
    }

    if( toAxis.z === -1 ) {
      tmpQuat.setFromAxisAngle( AxisX, SPI );
    }

    return tmpQuat;

  }

  update(){}

  dispose() {

    super.dispose();

    if( this.world && this.constraint ) {

      this.world.removeConstraint( this.constraint );

    }
  }

}

 // 铰链约束
 class A3HingeConstraint extends A3Constraint {
  name = 'hinge_constraint';

  visible = true;
  hingeSize = .1;
  hingeColor = 0x1982cc;

  // 速度相关
  velocity = 0.0;
  impulse = SPI;

  maxImpulse = DPI;
  maxSpeed = SPI;
  maxAcceleration = SPI;

  // 运动相关
  targetCoil = 0.0;
  targetAngle = 0.0;
  targetSpeed = 0.0;
  acceleration = 0.0;

  accuracy = SPI / 180;

  startAngle = 0.0;
  targetAngle = 0.0;
  startTime = 0.0;
  passTime = 0.0;

  needsUpdate = false;

  // 角度限制
  limit = [ -DPI, DPI ];

  constructor( bodyA, bodyB ) {

    super( bodyA, bodyB );    
    this.createVisual();

  }

  createVisual() {

    this.threeObject = new THREE.Group();

    const hingeGeometry = new THREE.CylinderGeometry( this.hingeSize, this.hingeSize, this.hingeSize * 2 );

    const visual = new THREE.Mesh( 
      hingeGeometry, 
      new THREE.MeshPhongMaterial( { color: this.hingeColor, transparent: true, opacity: 0.8 } )
    );

    visual.quaternion.setFromAxisAngle( AxisX, HPI );

    this.threeObject.add( visual );

    this.enalbeAxes( this.visible, this.hingeSize * 2 );

  }

  enableConstraint( enable=true, velocity=0 ) {

    super.enableConstraint( enable );

    if( enable && !isNull( velocity ) ) {

      if( !isNull( this.constraint ) ) {

        this.setHingeVelocity( velocity );

      }

    }

  }

  createConstraint(){

    this.constraint = createHingeConstraint( 
      this.bodyA.physicsBody,
      this.bodyB.physicsBody,
      this.frameInA, this.frameInB,
      this.useReferenceFrameA
    );

  }

  setFromFrame( frameInA, frameInB ) {

    if( isNull(frameInA) ) {
      frameInA = this.frameInA;
    }
    else{
      this.frameInA = frameInA;
    }

    if( isNull( frameInB ) ) {
      frameInB = this.frameInB;
    }
    else{
      this.frameInB = frameInB;
    }

    this.createConstraint();

  }

  // 根据目标角度和当前角度计算速度
  calculateVelocity() {

    var currAngle = this.getHingeAngle();

    if( Math.abs( currAngle ) < this.accuracy ) {
      currAngle = 0.0;
    }

    currAngle = currAngle >= 0 ? currAngle : DPI + currAngle; // to [0, 2 * PI]

    const diffAngle = this.targetAngle - currAngle;

    if( Math.abs( diffAngle ) <= this.accuracy ) {

      this.velocity = 0.0
      this.impulse = this.maxImpulse;

      const endTime = performance.now();
      console.log( `Use time: ${endTime - this.startTime}s` );

      this.needsUpdate = false;
      
      return;

    }

    // console.log("Update velocity:",currAngle, this.targetAngle, diffAngle );

    if( diffAngle > 0 ) {
      this.velocity = this.targetSpeed;
    }
    else {
      this.velocity = -this.targetSpeed;
    }

  }

  // 使用锚点和旋转轴设置约束
  setFromPivot( pivotInA, pivotInB, axisInA, axisInB ) {
    const frameInA = this.frameInA;
    const frameInB = this.frameInB;

    if( isNull( pivotInA ) ) {
      pivotInA = new THREE.Vector3( 0, 0, 0 );
    }

    if( isNull( pivotInB ) ) {
      pivotInB = new THREE.Vector3( 0, 0, 0 );
    }

    let quatA, quatB;
    
  
    if( !isNull(axisInA) ){
      
      quatA = this.calculateQuaternion( axisInA );

    }

    if( !isNull(axisInB) ){
      
      quatB = this.calculateQuaternion( axisInB );

    }

    updateTransform( frameInA, pivotInA, quatA );
    updateTransform( frameInB, pivotInB, quatB );

    // this.setFromFrame();

  }

  // 获取关节角度
  getHingeAngle() {

    const threeObjA = this.bodyA.threeObject;
    const threeObjB = this.bodyB.threeObject;

    const quatA = threeObjA.quaternion.clone();
    const quatB = threeObjB.quaternion.clone();
    const tmpQuat = new THREE.Quaternion();

    let quat;

    quat = this.frameInA.getRotation();
    tmpQuat.set( quat.x(), quat.y(), quat.z(), quat.w() );
    quatA.multiply( tmpQuat );

    quat = this.frameInB.getRotation();
    tmpQuat.set( quat.x(), quat.y(), quat.z(), quat.w() );
    quatB.multiply( tmpQuat );

    const X = quatA.invert().multiply( quatB );

    const euler = new THREE.Euler().setFromQuaternion( X , 'ZXY');

    return euler.z;

  }

  // 设置角度
  setHingeAngle( angle ) {
    
    const threeObjA = this.bodyA.threeObject;
    // const threeObjB = this.bodyB.threeObject;

    const quatA = threeObjA.quaternion.clone();
    // const quatB = threeObjB.quaternion.clone();
    const tmpQuat = new THREE.Quaternion();

    let quat;

    const X = new THREE.Quaternion().setFromAxisAngle( AxisZ, angle );

    quat = this.frameInA.getRotation();
    tmpQuat.set( quat.x(), quat.y(), quat.z(), quat.w() );
    quatA.multiply( tmpQuat );

    quat = this.frameInB.getRotation();
    tmpQuat.set( quat.x(), quat.y(), quat.z(), quat.w() );
    // quatB.multiply( tmpQuat );

    quatA.multiply(X).multiply( tmpQuat );

    this.bodyB.setQuaternion( quatA );

  }

  // 移动至角度
  moveTo( angle, speed, acceleration) {

    this.targetCoil = Math.floor( angle / DPI );
    angle = angle % DPI;

    this.targetAngle = angle > 0 ? angle : DPI + angle;

    // 确保速度和加速度为正
    speed = Math.abs(speed);
    acceleration = Math.abs(acceleration);
    
    if ( isNull(speed) || speed > this.maxSpeed ) {
      this.targetSpeed = this.maxSpeed;
    }
    else {
      this.targetSpeed = speed;
    }

    if ( isNull( acceleration) || acceleration > this.maxAcceleration ){
      this.acceleration = this.maxAcceleration;
    }
    else {
      this.acceleration = acceleration;
    }
    
    this.startTime = performance.now();

    this.needsUpdate = true;

  }

  // 设置速度
  setHingeVelocity( velocity, impulse ) {

    this.velocity = isNull( velocity )? 0 : velocity;
    this.impulse  = isNull( impulse ) ? this.maxImpulse : impulse;

    this.needsUpdate = false;
  }

  // 停止
  stopHinge( impulse ) {

    this.velocity = 0.0;

    if( !isNull( impulse ) ) {
      this.impulse = impulse;
    }

    this.needsUpdate = false;

  }

  update() {

    if( isNull(this.scene) || isNull(this.world) || isNull(this.constraint) ) return;

    if( this.visible ) {
      
      // update hinge visual
      this.position.copy( this.bodyA.position );
      this.quaternion.copy( this.bodyA.quaternion );

      const p = this.frameInA.getOrigin();
      const q = this.frameInA.getRotation();

      const tmpPos = new THREE.Vector3( p.x(), p.y(), p.z() );
      const tmpQuat = new THREE.Quaternion( q.x(), q.y(), q.z(), q.w() );

      tmpPos.applyQuaternion( this.quaternion );
      this.quaternion.multiply( tmpQuat );

      this.position.add( tmpPos );

      this.threeObject.position.copy( this.position );
      this.threeObject.quaternion.copy( this.quaternion );
    }

    this.threeObject.visible = this.showHinge;

    if( this.needsUpdate ) {

      this.calculateVelocity();

    }

    this.constraint.enableAngularMotor( true, this.velocity, this.impulse );

  }

}

// 6Dof约束
class A36DofConstraint extends A3Constraint {
  
  linearLowerLimit = new THREE.Vector3(0, 0, 0);
  linearUpperLimit = new THREE.Vector3(-1, -1, -1);

  angularLowerLimit = new THREE.Vector3(  );
  angularUpperLimit = new THREE.Vector3( -1, -1, -1 );

  createConstraint(){
    
    this.constraint = create6DofConstraint(
      this.bodyA.physicsBody,
      this.bodyB.physicsBody,
      this.frameInA, this.frameInB,
      this.useReferenceFrameA
    )

    this.updateLimits();

  }

  updateLimits(){

    this.setLinearLowerLimit( this.linearLowerLimit );
    this.setLinearUpperLimit( this.linearUpperLimit );
    this.setAngularLowerLimit( this.angularLowerLimit );
    this.setAngularUpperLimit( this.angularUpperLimit );
    
  }

  setLinearLowerLimit( limit ) {

    if( !isNull( this.constraint ) ) {
      this.constraint.setLinearLowerLimit( toAmmoType( limit ) );
    }

  }

  setLinearUpperLimit( limit ) {

    if( !isNull( this.constraint ) ) {
      this.constraint.setLinearUpperLimit( toAmmoType( limit ) );
    }

  }

  setAngularLowerLimit( limit ) {

    if( !isNull( this.constraint ) ) {
      this.constraint.setAngularLowerLimit( toAmmoType( limit ) );
    }

  }

  setAngularUpperLimit( limit ) {

    if( !isNull( this.constraint ) ) {
      this.constraint.setAngularUpperLimit( toAmmoType( limit ) );
    }

  }

}

// 滑动约束
class A3SlideConstraint extends A36DofConstraint {

  linearLowerLimit = 0;
  linearUpperLimit = 0;

  angularLowerLimit = 0;
  angularUpperLimit = 0;

  createConstraint(){
    
    this.constraint = createSliderConstraint(
      this.bodyA.physicsBody,
      this.bodyB.physicsBody,
      this.frameInA, this.frameInB,
      this.useReferenceFrameA
    )

    this.updateLimits();

  }

  setLinearLowerLimit( limit ) {

    if( !isNull( this.constraint ) ) {
      this.constraint.setLowerLinLimit( limit );
    }

  }

  setLinearUpperLimit( limit ) {

    if( !isNull( this.constraint ) ) {
      this.constraint.setUpperLinLimit( limit );
    }

  }

  setAngularLowerLimit( limit ) {

    if( !isNull( this.constraint ) ) {
      this.constraint.setLowerAngLimit( limit );
    }

  }

  setAngularUpperLimit( limit ) {

    if( !isNull( this.constraint ) ) {
      this.constraint.setUpperAngLimit( limit );
    }

  }

  setMotorVelocity( velocity ) {
    
    if( !isNull( this.constraint ) ) {
      this.constraint.setTargetLinMotorVelocity( velocity );
    }

  }

  enableLinMotor( enalbe, velocity, force ) {
    if( !isNull( this.constraint ) ) {
      this.constraint.setTargetLinMotorVelocity( velocity );
      this.constraint.setMaxLinMotorForce( force );
      this.constraint.setPoweredLinMotor( enalbe );
    }
  }
}

export {
  A3HingeConstraint,
  A36DofConstraint,
  A3SlideConstraint,
}